//
// Created by cybaster on 25-6-8.
//

#pragma once

#include <ocs2_self_collision/SelfCollisionConstraint.h>

#include "ocs2_humanoid_robot/LeggedRobotPreComputation.h"

namespace ocs2 {
namespace legged_robot {

class LeggedSelfCollisionConstraint final : public SelfCollisionConstraint {
public:
  LeggedSelfCollisionConstraint(
      const CentroidalModelPinocchioMapping &mapping,
      PinocchioGeometryInterface pinocchioGeometryInterface,
      scalar_t minimumDistance)
      : SelfCollisionConstraint(mapping, std::move(pinocchioGeometryInterface),
                                minimumDistance) {}
  ~LeggedSelfCollisionConstraint() override = default;
  LeggedSelfCollisionConstraint(const LeggedSelfCollisionConstraint &rhs) =
      default;
  LeggedSelfCollisionConstraint *clone() const override {
    return new LeggedSelfCollisionConstraint(*this);
  }

protected:
  const PinocchioInterface &
  getPinocchioInterface(const PreComputation &preComputation) const override {
    return cast<LeggedRobotPreComputation>(preComputation)
        .getPinocchioInterface();
  }
};
} // namespace legged_robot
} // namespace ocs2
